// include/paths/path1_waypoints.hpp
#pragma once
#include <vector>
#include <geometry_msgs/msg/pose.hpp>

namespace paths {

// 一条通用的工具函数：用 {px,py,pz, ox,oy,oz,ow} 填一个 Pose
inline geometry_msgs::msg::Pose makePose(double px, double py, double pz,
                                         double ox, double oy, double oz, double ow) {
  geometry_msgs::msg::Pose p;
  p.position.x = px; p.position.y = py; p.position.z = pz;
  p.orientation.x = ox; p.orientation.y = oy; p.orientation.z = oz; p.orientation.w = ow;
  return p;
}

// 路径1（就是你现在函数里的那批点）
inline std::vector<geometry_msgs::msg::Pose> path1_waypoints() {
  std::vector<geometry_msgs::msg::Pose> wps;
  wps.reserve(12);

  // 目标点0 28/-45/90/-45/0
  wps.push_back(makePose( 0.083024, 0.10987, 0.28763,  0,        0,        0.24475, 0.96959));
  // 目标点1 28/-50/90/15/0
  wps.push_back(makePose(-0.034461, 0.046522, 0.10748, -0.11163, 0.44231,  0.21777, 0.86283));
  // 目标点2 28/-60/100/10/0
  wps.push_back(makePose(-0.033193, 0.047216, 0.096255,-0.10337, 0.40969,  0.22182, 0.87879));
  // 目标点3 28/-65/110/0/0
  wps.push_back(makePose(-0.016352, 0.056293, 0.087686,-0.093294,0.36973,  0.22624, 0.89633));
  // 目标点4 28/-70/119/-9/0
  wps.push_back(makePose(-0.00072952,0.064723,0.083604,-0.082545,0.32711,  0.23041, 0.91274));
  // 目标点5 28/-78/134/-28/0
  wps.push_back(makePose( 0.030862, 0.081752, 0.082453,-0.059886,0.23743,  0.23731, 0.94007));
  // 目标点55 28/-78/136/-37/0
  wps.push_back(makePose( 0.045914, 0.08981,  0.10271, -0.045323,0.17931,  0.24037, 0.9529 ));
  // 目标点6  28/-78/138/-44/0
  wps.push_back(makePose( 0.054854, 0.094613, 0.11793, -0.034817,0.13775,  0.24206, 0.9598 ));
  // 目标点7  28/-82/142/-52/0 半举平
  wps.push_back(makePose( 0.061581, 0.098223, 0.14013, -0.017288,0.068398, 0.24388, 0.96723));
  // 目标点8  28/-82/145/-58/0 举平
  wps.push_back(makePose( 0.067408, 0.10135,  0.14599, -0.010242,0.040522, 0.24425, 0.96881));
  // 目标点9  28/-90/142/-50/0 刮板
  wps.push_back(makePose( 0.053625, 0.093921, 0.16204, -0.0035733,0.014151,0.24446, 0.96955));
  // 目标点10 14/-90/142/-50/0 左移
  wps.push_back(makePose( 0.071307, 0.047553, 0.16428, -0.0011452,0.0093179,0.12094, 0.99262));
  // 目标点11 visul low-middle
  wps.push_back(makePose( 0.11027,  0,       0.226,    0,        0,        0,       1));

  return wps;
}

} // namespace paths
